
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       srv_hal_mgr.c
  * @author     baiyang
  * @date       2021-12-28
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "srv_hal_mgr.h"

#if defined(HAL_WITH_IO_MCU)
#include <iomcu/gp_iomcu.h>
#endif

#if defined(RT_USING_PWM)
#include <board/fmu_pwm_group_config.h>
#endif

#include <common/time/gp_time.h>
#include <common/console/console.h>
#include <common/gp_math/gp_mathlib.h>
#include <srv_channel/srv_channel.h>
/*-----------------------------------macro------------------------------------*/
#if !defined(HAL_PWM_GROUPS)
#define HAL_PWM_GROUPS 0
#endif

#define CHAN_DISABLED 255

#if !defined(HAL_PWM_FIRST_REAL_CHAN)
#define HAL_PWM_FIRST_REAL_CHAN 1
#endif
/*----------------------------------typedef-----------------------------------*/
#if defined(RT_USING_PWM)
struct pwm_group {
        const char pwm_group_name[10];

        struct rt_device_pwm* pwm_group_dev;

        // only advanced timers can do high clocks needed for more than 400Hz
        uint8_t chan[4]; // chan number, zero based, 255 for disabled

        // PWM驱动输出是否使能标志
        // 此标志用于设置PWM输出时，避免多次调用rt_pwm_enable函数
        bool pwm_chan_enable[4];

        // below this line is not initialised by hwdef.h
        enum output_mode current_mode;

        // mask of channels that are able to be enabled
        uint16_t ch_mask;
        // mask of channels that are enabled and active
        uint16_t en_mask;

        bool pwm_started;
        uint32_t rc_frequency;
        uint32_t rc_period_ns;
};
#endif
/*---------------------------------prototype----------------------------------*/
#if defined(RT_USING_PWM)
void srv_hal_set_freq_group(struct pwm_group *group);
struct pwm_group *srv_hal_find_chan(uint8_t chan, uint8_t *group_idx);
#endif
void srv_hal_push_local(void);
/*----------------------------------variable----------------------------------*/
#if defined(RT_USING_PWM)
static struct pwm_group pwm_group_list[] = {HAL_PWM_GROUPS};
static const uint8_t NUM_GROUPS = ARRAY_SIZE(pwm_group_list);
#endif

static srv_hal_mgr srv_mgr;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void srv_hal_ctor()
{
    if (srv_mgr._initialised) {
        // cannot init RCOutput twice
        return;
    }

#if defined(HAL_WITH_IO_MCU)
    if (brd_io_enabled()) {
        // with IOMCU the local (FMU) channels start at 8
        srv_mgr.chan_offset = 8;
    }
#endif

    srv_mgr.iomcu_mode = MODE_PWM_NORMAL;

#if defined(RT_USING_PWM)
    for (uint8_t i = 0; i<NUM_GROUPS; i++) {
        pwm_group_list[i].pwm_group_dev = (struct rt_device_pwm *)rt_device_find(pwm_group_list[i].pwm_group_name);

        if (pwm_group_list[i].pwm_group_dev == NULL) {
            continue;
        }

        //Start Pwm groups
        pwm_group_list[i].current_mode = MODE_PWM_NORMAL;

        for (uint8_t j = 0; j < 4; j++ ) {
            if (srv_channels_is_GPIO(pwm_group_list[i].chan[j]+srv_mgr.chan_offset)) {
                pwm_group_list[i].chan[j] = CHAN_DISABLED;
            } 

            if (pwm_group_list[i].chan[j] != CHAN_DISABLED) {
                srv_mgr.num_fmu_channels = MAX(srv_mgr.num_fmu_channels, pwm_group_list[i].chan[j]+1);
                pwm_group_list[i].ch_mask |= (1U<<pwm_group_list[i].chan[j]);

                if ((((rt_device_t)pwm_group_list[i].pwm_group_dev)->open_flag & RT_DEVICE_OFLAG_OPEN) != RT_DEVICE_OFLAG_OPEN) {
                    rt_device_open((rt_device_t)pwm_group_list[i].pwm_group_dev, RT_DEVICE_OFLAG_RDWR);
                }
                // stm32 PWM驱动通道值从1开始
                rt_pwm_disable(pwm_group_list[i].pwm_group_dev, j + HAL_PWM_FIRST_REAL_CHAN);
                pwm_group_list[i].pwm_chan_enable[j] = false;
            }
        }
        if (pwm_group_list[i].ch_mask != 0) {
            pwm_group_list[i].pwm_started = true;
        }
    }
#endif

#if defined(HAL_WITH_IO_MCU)
    iomcu_ctor();
    if (brd_io_enabled()) {
        iomcu_init();
    }
#endif

    // setup default output rate of 50Hz
    srv_hal_set_freq(0xFFFF ^ ((1U<<srv_mgr.chan_offset)-1), 50);

#if defined(HAL_GPIO_PIN_SAFETY_IN)
    srv_mgr._safety_state = SAFETY_DISARMED;
#endif

    srv_mgr._initialised = true;
}

/**
  * @brief       
  * @param[in]   
  * @param[out]  
  * @retval      
  * @note        
  */
enum safety_state srv_hal_safety_switch_state(void)
{
#if defined(HAL_WITH_IO_MCU)
    if (brd_io_enabled()) {
        srv_mgr._safety_state = iomcu_get_safety_switch_state();
    }
#endif

    return srv_mgr._safety_state;
}

/*
  force the safety switch on, disabling PWM output from the IO board
*/
bool srv_hal_force_safety_on(void)
{
#if defined(HAL_WITH_IO_MCU)
    if (brd_io_enabled()) {
        return iomcu_force_safety_on();
    }
    return false;
#else
    srv_mgr._safety_state = SAFETY_DISARMED;
    return true;
#endif
}

/*
  force the safety switch off, enabling PWM output from the IO board
*/
void srv_hal_force_safety_off(void)
{
#if defined(HAL_WITH_IO_MCU)
    if (brd_io_enabled()) {
        iomcu_force_safety_off();
    }
#else
    srv_mgr._safety_state = SAFETY_ARMED;
#endif
}

/*
  update safety state
 */
void srv_hal_safety_update(void)
{
    uint32_t now = time_millis();
    if (now - srv_mgr.safety_update_ms < 100) {
        // update safety at 10Hz
        return;
    }
    srv_mgr.safety_update_ms = now;

    // remember mask of channels to allow with safety on
    srv_mgr.safety_mask = brd_get_safety_mask();

#if defined(HAL_GPIO_PIN_SAFETY_IN)
    // handle safety button
    bool safety_pressed = rt_pin_read(HAL_GPIO_PIN_SAFETY_IN);
    bool safety_pressed = false;
    if (safety_pressed) {
        if (srv_mgr.safety_press_count < 255) {
            srv_mgr.safety_press_count++;
        }
        if (brd_safety_button_handle_pressed(srv_mgr.safety_press_count)) {
            if (srv_mgr._safety_state ==SAFETY_ARMED) {
                srv_mgr._safety_state = SAFETY_DISARMED;
            } else {
                srv_mgr._safety_state = SAFETY_ARMED;
            }
        }
    } else {
        srv_mgr.safety_press_count = 0;
    }
#elif defined(HAL_WITH_IO_MCU)
    srv_mgr._safety_state = srv_hal_safety_switch_state();
    iomcu_set_safety_mask(srv_mgr.safety_mask);
#endif

#if defined(HAL_GPIO_PIN_LED_SAFETY)
    srv_mgr.led_counter = (srv_mgr.led_counter+1) % 16;
    const uint16_t led_pattern = safety_state==SAFETY_DISARMED?0x5500:0xFFFF;
    rt_pin_write(HAL_GPIO_PIN_LED_SAFETY, (led_pattern & (1U << srv_mgr.led_counter))?0:1);
#endif
}

/*
  start corking output
 */
void srv_hal_cork(void)
{
    srv_mgr.corked = true;
#if defined(HAL_WITH_IO_MCU)
    if (brd_io_enabled()) {
        iomcu_cork();
    }
#endif
}

/*
  stop corking output
 */
void srv_hal_push(void)
{
    srv_mgr.corked = false;
    srv_hal_push_local();
#if defined(HAL_WITH_IO_MCU)
    if (brd_io_enabled()) {
        iomcu_push();
    }
#endif
}

void srv_hal_write(uint8_t chan, uint16_t period_us)
{
    if (chan >= max_channels) {
        return;
    }
    srv_mgr.last_sent[chan] = period_us;

#if defined(HAL_WITH_IO_MCU)
    // handle IO MCU channels
    if (brd_io_enabled()) {
        uint16_t io_period_us = period_us;
        if ((srv_mgr.iomcu_mode == MODE_PWM_ONESHOT125) && ((1U<<chan) & srv_mgr.io_fast_channel_mask)) {
            // the iomcu only has one oneshot setting, so we need to scale by a factor
            // of 8 here for oneshot125
            io_period_us /= 8;
        }
        iomcu_write_channel(chan, io_period_us);
    }
#endif
    if (chan < srv_mgr.chan_offset) {
        return;
    }

    if (srv_mgr._safety_state == SAFETY_DISARMED && !(srv_mgr.safety_mask & (1U<<chan))) {
        // implement safety pwm value
        period_us = 0;
    }

    chan -= srv_mgr.chan_offset;

    srv_mgr.period[chan] = period_us;
    srv_mgr.period_ns[chan] = (uint32_t)srv_mgr.period[chan] * 1000;

    if (chan < srv_mgr.num_fmu_channels) {
        srv_mgr.active_fmu_channels = MAX(chan+1, srv_mgr.active_fmu_channels);
        if (!srv_mgr.corked) {
            srv_hal_push_local();
        }
    }
}

/*
  push values to local channels from period[] array
 */
void srv_hal_push_local(void)
{
#if defined(RT_USING_PWM)
    if (srv_mgr.active_fmu_channels == 0) {
        return;
    }
    uint16_t outmask = (1U<<srv_mgr.active_fmu_channels)-1;
    outmask &= srv_mgr.en_mask;

    bool safety_on = srv_hal_safety_switch_state() == SAFETY_DISARMED;

    struct pwm_group* group = NULL;
    for (uint8_t i = 0; i<NUM_GROUPS; i++) {
        group = &pwm_group_list[i];
        if (group->pwm_group_dev == NULL) {
            continue;
        }

        if (!group->pwm_started) {
            continue;
        }
        for (uint8_t j = 0; j < 4; j++) {
            uint8_t chan = group->chan[j];
            if (!(group->chan[j] != CHAN_DISABLED && (group->en_mask & (1U << group->chan[j])))) {
                continue;
            }
            if (outmask & (1UL<<chan)) {
                uint32_t period_ns = srv_mgr.period_ns[chan];

                if (safety_on && !(srv_mgr.safety_mask & (1U<<(chan+srv_mgr.chan_offset)))) {
                    // safety is on, overwride pwm
                    period_ns = 0;
                }

                if (group->current_mode < MODE_PWM_DSHOT150) {
                    rt_pwm_set_pulse(group->pwm_group_dev, j + HAL_PWM_FIRST_REAL_CHAN, period_ns);
                }

                if (!group->pwm_chan_enable[j]) {
                    rt_pwm_enable(group->pwm_group_dev, j + HAL_PWM_FIRST_REAL_CHAN);
                    group->pwm_chan_enable[j] = true;
                }
            }
        }
    }
#endif
}

uint16_t srv_hal_read(uint8_t chan)
{
    if (chan >= max_channels) {
        return 0;
    }
#if defined(HAL_WITH_IO_MCU)
    if (chan < srv_mgr.chan_offset) {
        uint16_t period_us = iomcu_read_channel(chan);
        if ((srv_mgr.iomcu_mode == MODE_PWM_ONESHOT125) && ((1U<<chan) & srv_mgr.io_fast_channel_mask)) {
            // convert back to 1000 - 2000 range
            period_us *= 8;
        }
        return period_us;
    }
#endif
    chan -= srv_mgr.chan_offset;
    return srv_mgr.period[chan];
}

void srv_hal_read2(uint16_t* period_us, uint8_t len)
{
    if (len > max_channels) {
        len = max_channels;
    }
#if defined(HAL_WITH_IO_MCU)
    for (uint8_t i=0; i<MIN(len, srv_mgr.chan_offset); i++) {
        period_us[i] = iomcu_read_channel(i);
        if ((srv_mgr.iomcu_mode == MODE_PWM_ONESHOT125) && ((1U<<i) & srv_mgr.io_fast_channel_mask)) {
            // convert back to 1000 - 2000 range
            period_us[i] *= 8;
        }
    }
#endif
    if (len <= srv_mgr.chan_offset) {
        return;
    }
    len -= srv_mgr.chan_offset;
    period_us += srv_mgr.chan_offset;

    rt_memcpy(period_us, srv_mgr.period, len*sizeof(uint16_t));
}

/*
  enable sbus output
 */
bool srv_hal_enable_px4io_sbus_out(uint16_t rate_hz)
{
#if defined(HAL_WITH_IO_MCU)
    if (brd_io_enabled()) {
        return iomcu_enable_sbus_out(rate_hz);
    }
#endif
    return false;
}

/*
  set output frequency in HZ for a set of channels given by a mask
 */
void srv_hal_set_freq(uint32_t chmask, uint16_t freq_hz)
{
#if defined(HAL_WITH_IO_MCU)
    if (brd_io_enabled()) {
        // change frequency on IOMCU
        uint16_t io_chmask = chmask & 0xFF;
        if (io_chmask) {
            uint8_t count = iomcu_get_ch_masks_size();
            const uint8_t* ch_masks = iomcu_ch_masks();
            // disallow changing frequency of this group if it is greater than the default
            for (uint8_t i=0; i<count; i++) {
                const uint16_t mask = io_chmask & ch_masks[i];
                if (mask != 0) {
                    if (freq_hz > 50) {
                        srv_mgr.io_fast_channel_mask |= mask;
                    } else {
                        srv_mgr.io_fast_channel_mask &= ~mask;
                    }
                }
            }
            iomcu_set_freq(srv_mgr.io_fast_channel_mask, freq_hz);
        }
    }
#endif

    // convert to a local (FMU) channel mask
    chmask >>= srv_mgr.chan_offset;
    if (chmask == 0) {
        return;
    }

    /*
      we enable the new frequency on all groups that have one
      of the requested channels. This means we may enable high
      speed on some channels that aren't requested, but that
      is needed in order to fly a vehicle such as a hex
      multicopter properly
    */
#if defined(RT_USING_PWM)
    for (uint8_t i = 0; i<NUM_GROUPS; i++) {
        if (pwm_group_list[i].pwm_group_dev == NULL) {
            continue;
        }

        // greater than 400 doesn't give enough room at higher periods for
        // the down pulse. This still allows for high rate with oneshot and dshot.
        uint16_t group_freq = freq_hz;
        if (group_freq > 400 && pwm_group_list[i].current_mode != MODE_PWM_BRUSHED) {
            group_freq = 400;
        }
        if ((pwm_group_list[i].ch_mask & chmask) != 0 && pwm_group_list[i].rc_frequency != group_freq) {
            pwm_group_list[i].rc_frequency = group_freq;
            pwm_group_list[i].rc_period_ns = 1000000000/pwm_group_list[i].rc_frequency;
            srv_hal_set_freq_group(&pwm_group_list[i]);
            // disallow changing frequency of this group if it is greater than the default
            if (group_freq > 50) {
                srv_mgr.fast_channel_mask |= pwm_group_list[i].ch_mask;
            }
        }
    }
#endif

}

#if defined(RT_USING_PWM)
/*
  setup the output frequency for a group and start pwm output
 */
void srv_hal_set_freq_group(struct pwm_group *group)
{
    for (uint8_t j=0; j<4; j++) {
        if (group->pwm_group_dev == NULL) {
            break;
        }

        if (group->chan[j] != CHAN_DISABLED) {
            // 周期、脉宽单位是纳秒
            rt_pwm_set_period(group->pwm_group_dev, j + HAL_PWM_FIRST_REAL_CHAN, group->rc_period_ns);
        }
    }
}
#endif

/*
  set default output rate
 */
void srv_hal_set_default_rate(uint16_t freq_hz)
{
#if defined(HAL_WITH_IO_MCU)
    if (brd_io_enabled()) {
        iomcu_set_default_rate(freq_hz);
    }
#endif

#if defined(RT_USING_PWM)
    for (uint8_t i = 0; i<NUM_GROUPS; i++) {
        if (pwm_group_list[i].pwm_group_dev == NULL) {
            continue;
        }

        if ((pwm_group_list[i].ch_mask & srv_mgr.fast_channel_mask) || pwm_group_list[i].ch_mask == 0) {
            // don't change fast channels
            continue;
        }

        if (pwm_group_list[i].pwm_started && freq_hz != pwm_group_list[i].rc_frequency) {
            pwm_group_list[i].rc_frequency = freq_hz;
            pwm_group_list[i].rc_period_ns = 1000000000/pwm_group_list[i].rc_frequency;

            for (uint8_t j=0; j<4; j++) {
                if (pwm_group_list[i].chan[j] != CHAN_DISABLED) {
                    // 周期、脉宽单位是纳秒
                    rt_pwm_set_period(pwm_group_list[i].pwm_group_dev, j + HAL_PWM_FIRST_REAL_CHAN, pwm_group_list[i].rc_period_ns);
                }
            }
        }
    }
#endif
}

#if defined(RT_USING_PWM)
/*
  find pwm_group and index in group given a channel number
  通道数从1开始
 */
struct pwm_group *srv_hal_find_chan(uint8_t chan, uint8_t *group_idx)
{
    if (chan >= max_channels) {
        return NULL;
    }
    if (chan < srv_mgr.chan_offset) {
        return NULL;
    }
    chan -= srv_mgr.chan_offset;

    for (uint8_t i = 0; i<NUM_GROUPS; i++) {
        if (pwm_group_list[i].pwm_group_dev == NULL) {
            continue;
        }

        for (uint8_t j = 0; j < 4; j++) {
            if (pwm_group_list[i].chan[j] == chan) {
                *group_idx = j;
                return &pwm_group_list[i];
            }
        }
    }
    return NULL;
}
#endif

/* Output freq (1/period) control */
uint16_t srv_hal_get_freq(uint8_t chan)
{
#if defined(HAL_WITH_IO_MCU)
    if (chan < srv_mgr.chan_offset) {
        return iomcu_get_freq(chan);
    }
#endif

#if defined(RT_USING_PWM)
    uint8_t i;
    struct pwm_group *grp = srv_hal_find_chan(chan, &i);
    if (grp) {
        return grp->rc_frequency;
    }
#endif

    // assume 50Hz default
    return 50;
}

void srv_hal_enable_ch(uint8_t chan)
{
#if defined(RT_USING_PWM)
    uint8_t i;
    struct pwm_group *grp = srv_hal_find_chan(chan, &i);
    if (grp) {
        srv_mgr.en_mask |= 1U << (chan - srv_mgr.chan_offset);
        grp->en_mask |= 1U << (chan - srv_mgr.chan_offset);
    }
#endif
}

void srv_hal_disable_ch(uint8_t chan)
{
#if defined(RT_USING_PWM)
    uint8_t i;
    struct pwm_group *grp = srv_hal_find_chan(chan, &i);
    if (grp) {
        // 函数rt_pwm_disable的参数channel
        rt_pwm_disable(grp->pwm_group_dev, i + HAL_PWM_FIRST_REAL_CHAN);
        srv_mgr.en_mask &= ~(1U<<(chan - srv_mgr.chan_offset));
        grp->en_mask &= ~(1U << (chan - srv_mgr.chan_offset));
        grp->pwm_chan_enable[i] = false;
    }
#endif
}

/*
  setup output mode
 */
void srv_hal_set_output_mode(uint16_t mask, const enum output_mode mode)
{
#if defined(HAL_WITH_IO_MCU)
    if ((mode == MODE_PWM_ONESHOT ||
         mode == MODE_PWM_ONESHOT125) &&
        (mask & ((1U<<srv_mgr.chan_offset)-1)) &&
        brd_io_enabled()) {
        srv_mgr.iomcu_mode = mode;
        // also setup IO to use a 1Hz frequency, so we only get output
        // when we trigger
        iomcu_set_freq(srv_mgr.io_fast_channel_mask, 1);
        iomcu_set_oneshot_mode();
        return;
    }
    if (mode == MODE_PWM_BRUSHED &&
        (mask & ((1U<<srv_mgr.chan_offset)-1)) &&
        brd_io_enabled()) {
        srv_mgr.iomcu_mode = mode;
        iomcu_set_brushed_mode();
        return;
    }
#endif
}

/*
  set PWM to send to a set of channels if the FMU firmware dies
*/
void srv_hal_set_failsafe_pwm(uint32_t chmask, uint16_t period_us)
{
#if defined(HAL_WITH_IO_MCU)
    if (brd_io_enabled()) {
        iomcu_set_failsafe_pwm(chmask, period_us);
    }
#endif
}

void srv_hal_set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm)
{
    srv_mgr._esc_pwm_min = min_pwm;
    srv_mgr._esc_pwm_max = max_pwm;
}

/*------------------------------------test------------------------------------*/


